// 8/5 7:15
/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
 * Stack and tested in Gazebo SITL
 */
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/PositionTarget.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TwistStamped.h>
#include <math.h>
#include <std_srvs/Trigger.h>
#include <std_msgs/ColorRGBA.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf/transform_datatypes.h>
#include <mavros_msgs/PositionTarget.h>
#include <std_msgs/Int8.h> 
#include <std_msgs/Int16.h>
#include <std_msgs/Float32.h>
#include "std_msgs/Float32MultiArray.h"
#include "std_msgs/Float64MultiArray.h"
#include <std_msgs/String.h>
#include <stdlib.h>

#define TargeyHeight 1.5
#define TargetYaw 0.2
#define TargetRadius 0.5
#define Limitv_x 0.2
#define Limitv_y 0.2
#define Limitv_z 0.2
#define Limit_yaw 0.2

ros::Subscriber state_sub;//飞机当前状态cur
ros::Subscriber pose_sub;//飞机当前位置信息
ros::Subscriber vel_sub;//飞机速度信息
ros::Publisher local_pose_pub;//设置飞机目标位置（全局坐标
ros::Publisher vel_pub;//设置飞机速度（全局坐标
ros::Publisher state_pub;//飞机状态
ros::Publisher set_raw_pub; //以飞机自己为坐标系的速度控制
ros::Publisher state_message_pub;
ros::ServiceClient arming_client;
ros::ServiceClient set_mode_client;
geometry_msgs::TwistStamped vel_cmd; //全局消息，速度设置
geometry_msgs::PoseStamped pose, land_pose;//全局消息，分别用来控制飞机自身的位置与飞机降落add_executable(pole_test src/pole_test.cpp)
mavros_msgs::PositionTarget raw_pose; //全局消息，以飞机自己为坐标系

int land_distance = -1;
double last_error_yaw=0, last_error_dist=0;
double yaw_i = 0 , dist_i = 0;
bool cur_state;
int fire_flag = 0;

bool state_cb(std_srvs::Trigger::Request &req,std_srvs::Trigger::Response &res){  //用于通过命令启动飞机
    cur_state=!cur_state;
    // 显示请求数据
    ROS_INFO("Publish command [%s]", cur_state==true?"Yes":"No");
	// 设置反馈数据
	res.success = true;
	res.message = "Change drone command state!";
    return true;
}

double min(double a,double b){   //比较ab较小的一个数，其中a的值是被比较值，b是限制量（注意，不可用反
    double ret=0;
    if (a == 0){
        ret=0;
    }else if(fabs(a)>=fabs(b)){
        ret=fabs(b)*a/fabs(a);
    }else{
        ret=fabs(a)*a/fabs(a);
    }
    return ret;
}

// // 使大于180度的yaw角正常化，但此代码中没有用到
// double arc_normalize_yaw(double current_yaw){
//     double raw_yaw=current_yaw;
//     if(fabs(current_yaw)>3.14){
//         raw_yaw = current_yaw > 0 ? current_yaw - 6.28319 : current_yaw + 6.28319;
//     }
//     return raw_yaw;
// }

mavros_msgs::State current_state;  //订阅消息：当前状态
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

geometry_msgs::PoseStamped current_pose; //订阅消息：当前位置（世界坐标系）
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg) {
    current_pose = *msg;
}

geometry_msgs::TwistStamped current_vel; //订阅消息：当前速度
void vel_cb(const geometry_msgs::TwistStamped::ConstPtr& msg)
{
    //ROS_INFO("Current velocity: %.2f m/s", msg->twist.linear.x);
    current_vel = *msg;
}

// 这里将改成检测蓝色物块的
std_msgs::ColorRGBA fire_pose;
void fire_pose_cb(const std_msgs::ColorRGBA::ConstPtr& msg)
{
    fire_pose = *msg;
}

// 这里改成降落时候的圆的
std_msgs::ColorRGBA error_to_target;
void land_node_cb(const std_msgs::ColorRGBA::ConstPtr& msg)
{
    error_to_target = *msg;
}



class move_map
{
private:
    double kpx,kpy,kpz;
    double kix,kiy,kiz;
    double kdx,kdy,kdz;
    double range,base;
    double kp;
    double ki;
    double kd;
    double last_x, last_y, last_z;
    double last_vx, last_vy, last_vz;
    double ix, iy ,iz; 
    double acceleration_limit;

    double radius = 1.0;//半径
    double speed = 0.2;
    double angular_speed = speed / radius;
    double angle = 0.0;
    double rotation_angle;
    double last_angle = 0;

    int interupt_step = 15;
    double height = TargeyHeight;
    geometry_msgs::PoseStamped interupt_pose;


    double func(double error,double A,double B)//变速积分的子函数，主要就是对于积分的判断
    { 
        double result;
        if(error>=B+A){
            result = 0;
        }else if(error<B+A && error>B){
            result = (A+B-error)/A;
        }else{
            result = 1;
        }
        return result;
    }

    //下面的是PID_cal，PID统一计算函数
    //p,i,d为pid的参数，error为输入的误差，A，B为变速积分的参数；
    //B为开始积分的区域，A为区域（如果看不懂的话打我电话，或者全部输入0.5就行了）
    //serial number序号，每个地方应该不一样。
    double PID_cal(double p,double i,double d,double error,
        ros::Rate& rate,double A,double B,int serial_number)
    {
        static double last_error[100] = {0} ;
        static double err_i[100] = {0} ;
        double dt = rate.expectedCycleTime().toSec();
        err_i[serial_number] = error*func(error,A,B)*dt + err_i[serial_number];
        double err_d = (error-last_error[serial_number])*dt;
        double result = error*p + err_i[serial_number]*i + err_d*d;
        last_error[serial_number] = error;
        //ROS_INFO("err_i:%.1lf err_d:%.1lf func_i=%.1lf",err_i,err_d,func(error,A,B));
        return result;
    }


public:
    // 平移至目标位置的函数，其中target_pose的xyz为世界坐标系下的目标位置的xyz，返回值为0或1，0为未到达指定位置，1为到达
    // 使用示例：flag = move1.fly_to_target(target_position,vel_pub,rate);
    int fly_to_target(geometry_msgs::PoseStamped target_pose,ros::Publisher& local_vel_pub,ros::Rate& rate) 
    {
        int judge=0;
        // 初始化PID控制器
        double ex = 0.0, ey = 0.0, ez = 0.0;     
        // 计算PID控制器输出
        double dt = rate.expectedCycleTime().toSec();
        ex = target_pose.pose.position.x - current_pose.pose.position.x;
        ey = target_pose.pose.position.y - current_pose.pose.position.y;
        ez = target_pose.pose.position.z - current_pose.pose.position.z;
        ix += ex * dt * func(ex,range,base);
        iy += ey * dt * func(ey,range,base);
        iz += ez * dt * func(ez,range,base);
        ix = min(ix,0.3);
        iy = min(iy,0.3);
        iz = min(iz,0.3);
        double dx = (ex - last_x) / dt;
        double dy = (ey - last_y) / dt;
        double dz = (ez - last_z) / dt;
        double vx = kp * ex + ki * ix + kd * dx;
        double vy = kp * ey + ki * iy + kd * dy;
        double vz = kpz * ez + kiz * iz + kdz * dz;
        ROS_INFO("vx = %.2lf vy = %.2lf vz = %.2lf",vx,vy,vz);
        last_x = ex;
        last_y = ey;
        last_z = ez;
        // 更新速度指令
        geometry_msgs::TwistStamped vel;
        vel.header.stamp = ros::Time::now();
        vel.twist.linear.x = min(vx,Limitv_x);
        vel.twist.linear.y = min(vy,Limitv_y);
        vel.twist.linear.z = min(vz,Limitv_z);
        vel.twist.angular.x = 0.0;
        vel.twist.angular.y = 0.0;
        vel.twist.angular.z = 0.0;
        // 发布速度指令 
        local_vel_pub.publish(vel);
        // 判断是否到达目标点
        if (fabs(ex) < 0.1 && fabs(ey) < 0.1 ) {
            judge = 1;
        }
        return judge;
    }

    void velocity_land(ros::Rate& rate)  //降落程序中，pid_cal的序号为0
    {
        mavros_msgs::PositionTarget raw_target;
        raw_target.coordinate_frame = 1;
        raw_target.type_mask = mavros_msgs::PositionTarget::IGNORE_PX | 
        mavros_msgs::PositionTarget::IGNORE_PY | 
        mavros_msgs::PositionTarget::IGNORE_PZ |
        mavros_msgs::PositionTarget::IGNORE_YAW;
        raw_target.velocity.x = -current_pose.pose.position.x;
        raw_target.velocity.y = -current_pose.pose.position.y;
        raw_target.velocity.z = -current_pose.pose.position.z ;
        //ROS_INFO("%.2lf",raw_target.velocity.z);
        set_raw_pub.publish(raw_target);
    }


    //巡线函数，已经调试完成
    void line_follower(std_msgs::ColorRGBA RGB,double limit_v_x, 
        double limit_v_y ,double limit_a, ros::Rate& rate)
    {  
        double rgbr = RGB.r;
        double rgba = RGB.g*3.1415/180;
        double kp = 0.5;
        double ki = 0.08;
        double kd = 0;
        double ir = 0.0, ia = 0.0;
        double er = 0.0, ea = 0.0;
        double last_r = 0.0, last_a = 0.0;      // 计算PID控制器输出
        double dt = rate.expectedCycleTime().toSec();
        er = min(rgbr,300);
        er = rgbr*0.1;
        //er = low_pass_fliter(er);
        ea = min(rgba,0.52);
        ir += er * dt;
        ia += ea * dt;
        double dr = (er - last_r) / dt;
        double da = (ea - last_a) / dt;
        double vr = kp * er + ki * ir + kd * dr;
        double va = kp * ea + ki * ia + kd * da;
        last_r = er;
        last_a = ea;
        double vx=0;
        double vy=0;
        if(ea == 0 || er == 0){
            vx = 0;
            vy = limit_v_y;
        }else{
            vy = fabs(vr/sin(2*ea)*sin(1.57075-3*ea));
            //vx = (-er)/fabs(sin(2*ea)*cos(1.57075-3*ea));
            vx = fabs(vy*tan(3*ea))*fabs(er)/(-er);
        }
        //double current_yaw = tf::getYaw(current_pose.pose.orientation);
        //ROS_INFO("b=%.2lf r=%.2lf g=%.2lf" ,py_data.b, py_data.r ,py_data.g);
        //set_speed_body(min(vx,limit_v),min(vy,limit_v),0,min(va,limit_a));
        pose_velocity_ctrl(vx,vy,limit_v_x,limit_v_y,0,0,rate);
        //velocity_ctrl(vx,vy,va,limit_v,limit_a,vel_pub); //-va*100
    }

    //对准圆心函数
    int circle_locator(std_msgs::ColorRGBA RGBA,ros::Publisher& local_vel_pub,ros::Rate& rate)
    {    
        int judge = 0;
        double err_x = -RGBA.g/300; 
        double err_y = -RGBA.r/300;
        ROS_INFO("err = %.2lf,err = %.2lf", err_x, err_y);
        double dt = rate.expectedCycleTime().toSec();
        double ez = height - current_pose.pose.position.z;
        ix += err_x * dt * func(err_x,range,base);
        iy += err_y * dt * func(err_y,range,base);
        iz += ez * dt * func(ez,range,base);
        ix = min(ix,0.1);
        iy = min(iy,0.1);
        iz = min(iz,0.1);
        double dx = (err_x - last_x) / dt;
        double dy = (err_y - last_y) / dt;
        double dz = (ez - last_z) / dt;
        double vx = kp * err_x + ki * ix + kd * dx;
        double vy = kp * err_y + ki * iy + kd * dy;
        double vz = kpz * ez + kiz * iz + kdz * dz;
        geometry_msgs::TwistStamped vel;
        vel.header.stamp = ros::Time::now();
        vel.twist.linear.x = min(vx,Limitv_x);
        vel.twist.linear.y = min(vy,Limitv_y);
        vel.twist.linear.z = min(vz,Limitv_z);
        ROS_INFO("vx = %.2lf,vy = %.2lf,vz = %.2lf", vel.twist.linear.x, vel.twist.linear.y, vel.twist.linear.z);
        vel.twist.angular.x = 0.0;
        vel.twist.angular.y = 0.0;
        vel.twist.angular.z = 0.0;
        // 发布速度指令 
        local_vel_pub.publish(vel);
        // 判断是否到达目标点
        return judge;
    }

    //基于速度的速度控制：设置飞机的速度，其中x,y,z都是飞机以自己为坐标系的速度，yaw_rate是偏航角旋转速率
    void set_speed_body(double x, double y, double z,  double yaw_rate ) 
    {
        mavros_msgs::PositionTarget raw_target;
        geometry_msgs::Twist vz0;
        raw_target.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_NED;
        raw_target.type_mask = mavros_msgs::PositionTarget::IGNORE_PX | 
        mavros_msgs::PositionTarget::IGNORE_PY | 
        //mavros_msgs::PositionTarget::IGNORE_PZ | 
        mavros_msgs::PositionTarget::IGNORE_AFX | 
        mavros_msgs::PositionTarget::IGNORE_AFY | 
        mavros_msgs::PositionTarget::IGNORE_AFZ | 
        mavros_msgs::PositionTarget::IGNORE_YAW;
        if (fabs(yaw_rate) < 1e-2)
        raw_target.type_mask |= mavros_msgs::PositionTarget::IGNORE_YAW_RATE;
        ROS_INFO("body_speed:%.1lf %.1lf %.1lf",x,y,yaw_rate);
        raw_target.velocity.x = x;
        raw_target.velocity.y = y;
        raw_target.velocity.z = z;
        raw_target.yaw_rate =yaw_rate;//yaw_rate * 0.01745329;
        set_raw_pub.publish(raw_target);
    }

    //基于位置的速度控制：设置飞机的速度，其中x,y都是飞机以世界为坐标系的速度，yaw_rate是偏航角旋转速率，z默认为目标高度
    void pose_velocity_ctrl(double vx, double vy, double vel_limit_x,double vel_limit_y, double yaw_rate, double yaw_limit, ros::Rate& rate )
    {   
        geometry_msgs::PoseStamped position=current_pose;
        // double current_time = ros::Time::now().toSec();
        // double elapsed_time = current_time - yaw_last_time;
        // yaw_last_time = current_time;
        double dt = rate.expectedCycleTime().toSec();
        vx=min(vx,vel_limit_x);
        vy=min(vy,vel_limit_y);
        yaw_rate = min(yaw_rate,yaw_limit);
        position.header.stamp = ros::Time::now();
        position.header.frame_id = "map";
        ROS_INFO(" vx = %.2lf vy = %.2lf " ,vx , vy);
        double current_yaw = tf::getYaw(current_pose.pose.orientation);// Set the new yaw angle
        double new_yaw = current_yaw + yaw_rate * dt;
        tf::Quaternion q_tf = tf::createQuaternionFromYaw(new_yaw);
        geometry_msgs::Quaternion current_q;
        tf::quaternionTFToMsg(q_tf, current_q);
        position.pose.position.x = current_pose.pose.position.x + vx*dt;
        position.pose.position.y = current_pose.pose.position.y + vy*dt;
        position.pose.position.z = TargeyHeight; 
        position.pose.orientation = current_q;
        local_pose_pub.publish(position);
    }

    // 悬停，不稳，谨慎使用
    void hold(ros::Publisher& local_vel_pub)
    {
        geometry_msgs::TwistStamped vel;
        vel.header.stamp = ros::Time::now();
        vel.twist.linear.x = 0;
        vel.twist.linear.y = 0;
        vel.twist.linear.z = 0;
        local_vel_pub.publish(vel);
    }

    int pole_rotation(geometry_msgs::PoseStamped center_pose,ros::Publisher& local_vel_pub,ros::Rate& rate)
    {
        int flag = 0;
        double dt = rate.expectedCycleTime().toSec();
        angle += dt * angular_speed;
        //ROS_INFO("target_angle = %.2lf",angle);
        double px = radius * cos(angle)+center_pose.pose.position.x;
        double py = radius * sin(angle)+center_pose.pose.position.y;
        if(sqrt((px - current_pose.pose.position.x)*(px - current_pose.pose.position.x)+
        (py - current_pose.pose.position.y)*(py - current_pose.pose.position.y)
        )>0.3){
            angle -= dt * angular_speed * 3;
            //ROS_INFO("angle too large tar_angle = %.2lf",angle);
            pose.pose.position.x = radius * cos(last_angle)+center_pose.pose.position.x;
            pose.pose.position.y = radius * sin(last_angle)+center_pose.pose.position.y;
            pose.pose.position.z = TargeyHeight;
            local_pose_pub.publish(pose);
        }else{
        pose.pose.position.x = radius * cos(angle)+center_pose.pose.position.x;
        pose.pose.position.y = radius * sin(angle)+center_pose.pose.position.y;
        pose.pose.position.z = TargeyHeight;
        last_angle = angle;
        local_pose_pub.publish(pose);
        }        
        if(fabs(angle - rotation_angle) < 0.05){
            ROS_INFO("angle = %.2lf , current_angle = %.2lf",angle,rotation_angle);
            flag = 1;
        }
        return flag;
    }

    int fire_global_interupt(int step)
    {
       if(fire_pose.a == 1 && fire_flag==0){
            last_x = 0.0, last_y = 0.0, last_z = 0.0;
            ix = 0.0, iy = 0.0, iz = 0.0; 
            last_vx = 0.0, last_vy = 0.0, last_vz = 0.0;
            interupt_step = step;
            interupt_pose = current_pose;
            step = 100;
            ROS_INFO("interupt %d",step);
            return step;
       }else{
            return step;
       }
    }

    void set_pid_xy(double p,double i,double d){
        kp = p;
        ki = i;
        kd = d;
    }

    void set_pid_z(double p,double i,double d){
        kpz = p;
        kiz = i;
        kdz = d;
    }

    void pid_init(){
        last_x = 0.0, last_y = 0.0, last_z = 0.0;
        ix = 0.0, iy = 0.0, iz = 0.0; 
        last_vx = 0.0, last_vy = 0.0, last_vz = 0.0;
    }

    void set_acceleration_limit(double a){
        acceleration_limit = a;
    }

    void print_pose(){
        ROS_INFO("px = %.2lf,py = %.2lf,pz = %.2lf",
        current_pose.pose.position.x,current_pose.pose.position.y,current_pose.pose.position.z);
    }

    void print_pid(){
        ROS_INFO("ix = %.2lf,iy = %.2lf,iz = %.2lf",
        ix,iy,iz);
    }    

    void set_height(double height_input){
        height = height_input;
    }

    int get_interupt_step(){
        return interupt_step;
    }

    geometry_msgs::PoseStamped get_interupt_pose(){
        return interupt_pose;
    }


    void circle_init(double r,double linear_vel,double current_angle,double spin_angle){
        radius = r;//半径
        speed = linear_vel;
        angular_speed = speed / radius;
        angle = current_angle;
        rotation_angle = spin_angle*min(linear_vel,0.1)*10 + current_angle;
        last_angle = current_angle;
        ROS_INFO("circle init: r= %.2lf,current_angle = %.2lf",r,current_angle);
    }
};

int main(int argc, char** argv)
{
    int step=1;
    double target_speed=0.5;
    int rotation_direction=0;
    std_msgs::ColorRGBA lidar_data_sender;
    double* pole_data;
    double yaw_angle,start_yaw;
    std_msgs::Int8 vision_cmd_sender;
    std_msgs::Int16 led_data_sender;
    led_data_sender.data = 0;
    vision_cmd_sender.data = 1;
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;
    // 订阅命令消息

    // 订阅飞行器状态和发布位置设定点
    state_sub = nh.subscribe<mavros_msgs::State>
        ("/mavros/state", 10, state_cb);  //订阅状态
    pose_sub = nh.subscribe<geometry_msgs::PoseStamped>
        ("/mavros/local_position/pose", 10, pose_cb); //订阅位置信息.
    vel_sub = nh.subscribe<geometry_msgs::TwistStamped> 
        ("/mavros/local_position/velocity", 10 ,vel_cb);
    local_pose_pub = nh.advertise<geometry_msgs::PoseStamped>
        ("/mavros/setpoint_position/local", 10);  //设置位置
    vel_pub = nh.advertise<geometry_msgs::TwistStamped>
        ("/mavros/setpoint_velocity/cmd_vel", 10);  //设定速度
    set_raw_pub = nh.advertise<mavros_msgs::PositionTarget>
        ("/mavros/setpoint_raw/local", 10); //以自身为坐标系的速度控制
    state_message_pub = nh.advertise<std_msgs::Int8>
        ("/offboard_node/state",10);
    // 定义服务客户端，用于解锁/上锁无人机和切换到离线控制模式
    arming_client = nh.serviceClient<mavros_msgs::CommandBool>
        ("/mavros/cmd/arming");   
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
        ("/mavros/set_mode");
    ros::ServiceClient land_client = nh.serviceClient<mavros_msgs::CommandTOL>
        ("/mavros/cmd/land");
    ros::ServiceServer state_client = nh.advertiseService
        ("/command",state_cb);    

    // 指定发布位置设定点的频率
    ros::Rate rate(50.0);  

    // 等待 FCU 连接
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    // 发布一些起始位置设定点，然后才开始执行控制指令
    geometry_msgs::TwistStamped vector;
    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        vel_pub.publish(vector);
        ros::spinOnce();
        rate.sleep();
    }

    // 请求进入离线控制模式
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    // 请求解锁飞行器
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    // 记录上一次请求时间
    ros::Time last_request = ros::Time::now();
    ros::Time last_time = ros::Time::now();
    ros::Time fire_time = ros::Time::now();
    mavros_msgs::CommandTOL land_cmd;
    std_msgs::Int8 offboard_state;
    std_msgs::Int8 drop_command;
    drop_command.data = 0;
    geometry_msgs::PoseStamped target_position;
    geometry_msgs::PoseStamped landing_position;
    geometry_msgs::PoseStamped rounding_pose[3];
    land_cmd.request.altitude = 0.0;
    land_cmd.request.yaw = 0.0;

    // 用于步骤
    int flag = 0;
  
    // 定义class的实例
    move_map move1;
    
    while(ros::ok())
    {
        ros::spinOnce(); 
		// ROS_INFO("Pose-z: %f",current_pose.pose.position.z);
		// 无人机状态设定与判断      
        // 进入while循环后，先循环5s，然后再向客户端发送无人机状态设置的消息
        // set_mode_client.call   arming_client.call 
        if( current_state.mode != "OFFBOARD" &&(ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&offb_set_mode.response.mode_sent){
                ROS_WARN("Offboard enabled");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_WARN("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
            else    //  无人机 Offboard enabled && Vehicle armed 后
            {        
                switch(step)
                {
                    case 1:  // case1 起飞程序
                        if(current_pose.pose.position.z < TargeyHeight-0.1)
                        {
                            move1.pose_velocity_ctrl(0,0,0,0,0,0,rate);
                            // flag = move1.fly_to_target(target_position,vel_pub,rate);
                            // local_pose_pub.publish(target_position);
                        }else{
                            flag = 0;
                            // 切换模式
                            last_time = ros::Time::now();
                            ROS_WARN("step2");
                            step = 2;
                            move1.pid_init();
                            move1.set_pid_xy(0.8,0.1,0.2);
                            move1.set_pid_z(0.8,0.0,0.0);
                            move1.set_height(TargeyHeight);
                            target_position.pose.position.x = 2.82;
                            target_position.pose.position.y = 0;
                            target_position.pose.position.z = 1.50;

                            // target_position = current_pose;  //用于原地降落
                            // target_position.pose.position.z = 0.5
                            //step = 100;
                        }
                        break;  
                    case 2:
                        if(flag == 0)
                        {
                            // move1.pose_velocity_ctrl(0,0,0,0,0,0,rate);
                            flag = move1.fly_to_target(target_position,vel_pub,rate);
                            local_pose_pub.publish(target_position);
                        }else{
                            flag = 0;
                            // 切换模式
                            last_time = ros::Time::now();
                            ROS_WARN("step3");
                            step = 3;
                            move1.pid_init();
                            move1.set_pid_xy(0.8,0.1,0.2);
                            move1.set_pid_z(0.8,0.0,0.0);
                            move1.set_height(TargeyHeight);
                            target_position.pose.position.x = 3.55;
                            target_position.pose.position.y = -0.43;
                            target_position.pose.position.z = 1.50;
                        }
                        break;
                    case 3:
                        if(flag == 0)
                        {
                            flag = move1.fly_to_target(target_position,vel_pub,rate);
                            last_time = ros::Time::now();
                        }else{
                            flag = 0;
                            // 切换模式
                            last_time = ros::Time::now();
                            ROS_WARN("step4");
                            step = 4;
                            move1.pid_init();
                            move1.set_pid_xy(0.8,0.1,0.2);
                            move1.set_pid_z(0.8,0.0,0.0);
                            move1.set_height(TargeyHeight);
                            target_position.pose.position.x=3.225;
                            target_position.pose.position.y=-1.165;     
                            target_position.pose.position.z=1.50;                       
                        }
                        break;
                    case 4:
                        if(flag == 0)
                        {
                            flag = move1.fly_to_target(target_position,vel_pub,rate);
                            last_time = ros::Time::now();
                        }else{
                            flag = 0;
                            // 切换模式
                            last_time = ros::Time::now();
                            ROS_WARN("step5");
                            step = 5;
                            move1.pid_init();
                            move1.set_pid_xy(0.8,0.1,0.2);
                            move1.set_pid_z(0.8,0.0,0.0);
                            move1.set_height(TargeyHeight);
                            target_position.pose.position.x=3.225;
                            target_position.pose.position.y=-1.48;     
                            target_position.pose.position.z=1.50;                       
                        }
                        break;
                    case 5:
                        if(flag == 0)
                        {
                            flag = move1.fly_to_target(target_position,vel_pub,rate);
                            last_time = ros::Time::now();
                        }else{
                            flag = 0;
                            // 切换模式
                            last_time = ros::Time::now();
                            ROS_WARN("step6");
                            step = 6;
                            move1.pid_init();
                            move1.set_pid_xy(0.8,0.1,0.2);
                            move1.set_pid_z(0.8,0.0,0.0);
                            move1.set_height(TargeyHeight);
                            target_position.pose.position.x=2.82;
                            target_position.pose.position.y=-2;     
                            target_position.pose.position.z=1.50;                      
                        }
                        break;
                    case 6:
                        if(flag == 0)
                        {
                            flag = move1.fly_to_target(target_position,vel_pub,rate);
                            last_time = ros::Time::now();
                        }else{
                            flag = 0;
                            // 切换模式
                            last_time = ros::Time::now();
                            ROS_WARN("step7");
                            step = 7;
                            move1.pid_init();
                            move1.set_pid_xy(0.8,0.1,0.2);
                            move1.set_pid_z(0.8,0.0,0.0);
                            move1.set_height(TargeyHeight);
                            target_position.pose.position.x=0;
                            target_position.pose.position.y=-2;     
                            target_position.pose.position.z=1.50;                    
                        }
                        break;
                    case 7:
                        if(flag == 0)
                        {
                            flag = move1.fly_to_target(target_position,vel_pub,rate);
                            last_time = ros::Time::now();
                        }else{
                             flag = 0;
                            // 切换模式
                            last_time = ros::Time::now();
                            ROS_WARN("step8");
                            step = 8;
                            move1.pid_init();
                            move1.set_pid_xy(0.8,0.1,0.2);
                            move1.set_pid_z(0.8,0.0,0.0);
                            move1.set_height(TargeyHeight);
                            target_position.pose.position.x=0;
                            target_position.pose.position.y=-2;     
                            target_position.pose.position.z=1.50;     
                        }
                        break;
                    case 8:   // 准备降落
                        offb_set_mode.request.custom_mode = "AUTO.LAND";
                        if (current_state.mode != "AUTO.LAND" && (ros::Time::now() - last_request > ros::Duration(5.0)))
                        {

                            if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
                            {
                                ROS_INFO("AUTO.LAND enabled");
                                step = 0;
                            }
                            last_request = ros::Time::now();
                        }
                        break;
                    default:
                        break;
                }
            }
        }
        rate.sleep();
    }
    return 0;
}
